#!/usr/bin/env python

# Copyright (c) 2018 Intel Labs.
# authors: German Ros (german.ros@intel.com)
#
# This work is licensed under the terms of the MIT license.
# For a copy, see <https://opensource.org/licenses/MIT>.

"""
1.延伸轨迹（初始化直接放入中心线也算延伸轨迹）
2.检查轨迹碰撞
2.1 交通规则
2.2 交通标志
2.3 障碍物
3.多次曲线计算新轨迹（控制点包括哪些？）
4.重新检查轨迹碰撞
5.输出轨迹
-----------------------------------------
1.撒点 另一个线程：
1.0 在当前轨迹前方撒一个点
1.0.1 可以撒，那么，速度不变，或者增加一个加速度（未达到限速）
1.0.2 不可以撒，那么，速度为0，准备 VL曲线计算

1.1 在当前轨迹左右各撒一个点（如果可能）
1.1.1 可以撒，速度不变，或者增加一个加速度
1.1.2 不可以撒

2.计算新轨迹 5次多项式
——（SL曲线，然后沿着SL曲线选择各个S点的速度终点速度选择限速，vl曲线）
2.1 碰撞打分
2.2 

3.输出结果
"""

import math
from collections import deque
from enum import Enum

import numpy as np
from pygame import transform

import carla
from agents.map.local_map import FrenetPoint, LocalRoadTreeDeque
from agents.navigation.controller import VehiclePIDController
from agents.navigation.stanley_controller import StanleyController
from agents.prediction.perception_prediction import PerceptPrediction
from agents.tools.bezier_curv import GetWaveBezier
from agents.tools.misc import (
    clip_angle, compute_distance, compute_magnitude_angle, distance_vehicle,
    draw_speed_with_tf, draw_transforms, draw_way, draw_waypoints,
    get_acceleration, get_curvature, get_nearest_location_index,
    get_nearest_waypoint_index, get_speed)
from agents.tools.vehicle import MotionStatus


"""
v = v0 + sqrt(2*k*sin(phy*s)*s)
"""

def sin_velocity_planner(current_velocity, end_velocity, s_length, s_size) -> (float, list):
    """
    基于加速度为sin曲线来计算速度规划，那么速度曲线为cos曲线（积分关系）
    @param current_velocity : 当前速度 km/h
    @param end_velocity : 规划终点速度 km/h
    @param s_length : frenet坐标下规划长度 m
    @param s_size : 速度序列长度
    @return max_acc : 最大加速度
    @return velocity_list : 速度序列
    """
    velocity_buffer = [current_velocity] * s_size
    max_acc = 0
    # k * cos(phy * 0) + bias = current_velocity
    # k * cos(phy * s_length) + bias = end_velocity
    # 令 phy * s_length = n * pi，即经过半个周期完成减速，
    # 因为 cos(0) = 1, cos(pi) = -1
    if current_velocity != end_velocity:
        bias = (current_velocity + end_velocity) / 2.0
        k = (current_velocity - end_velocity) / 2.0
        phy = np.pi / s_length
        s_buffer = np.linspace(0.0, s_length, num=s_size)
        velocity_buffer = k * np.cos(phy * s_buffer) + bias
        max_acc = k*k*phy/2.0
    return max_acc, velocity_buffer

def compute_velocity_with_curvature(lat_acc_limited:float, frenet_point:FrenetPoint) -> float:
    curv = frenet_point.kappa
    return math.sqrt(lat_acc_limited / curv)

class PlanBoxVertice:
    """
    障碍物box顶点描述，包含
    @index对应了本地路径中的点序号，从而得到s等参数, 
    @l，标明离local的横向距离
    @location，world.location
    """
    def __init__(self, local_index, l, location):
        self.local_index = local_index
        self.l = l
        self.location = location

class RoadOption(Enum):
    """
    RoadOption represents the possible topological configurations
    when moving from a segment of lane to other.
    """
    VOID = -1
    LEFT = 1
    RIGHT = 2
    STRAIGHT = 3
    LANEFOLLOW = 4
    CHANGELANELEFT = 5
    CHANGELANERIGHT = 6
    CROSSWALK = 7

class VehicleAction(Enum):
    """
    车辆行为：刹车、直行、左变道、右变道
    """
    BRAKE = -1
    STRAIGHT = 0
    LC2LEFT = 1
    LC2RIGHT = 2

class ADRTKPlanner(object):
    """
    RTKPlanner implements the basic behavior of following a trajectory
    of waypoints that is generated on-the-fly.
    The low-level motion of the vehicle is computed by using two PID controllers,
    one is used for the lateral control
    and the other for the longitudinal control (cruise speed).

    When multiple paths are available (intersections)
    this local planner makes a random choice.
    """

    # Minimum distance to target waypoint as a percentage
    # (e.g. within 80% of total distance)

    # FPS used for dt
    FPS = 20

    def __init__(self, agent):
        """
        :param agent: agent that regulates the vehicle
        :param vehicle: actor to apply to local planner logic onto
        """
        print('now local planner is AD_RTK_planner')

        self._vehicle = agent.vehicle
        self._sampling_resolution = agent._sampling_resolution
        self._dt = 1.0/self.FPS
        
        vehicle_physics_control = self._vehicle.get_physics_control()
        weels = vehicle_physics_control.wheels
        # self._front_centre = (weels[0].position + weels[1].position) / 100 / 2
        self._axis_length = compute_distance(weels[0].position, weels[2].position) / 100 # cm -> m
        self._max_steer_angle = weels[0].max_steer_angle / 2 # degree 假定内外角一致
        
        self._map = agent.vehicle.get_world().get_map()
        self._world = agent.vehicle.get_world()

        self._target_speed = None
        self.sampling_radius = None
        self._min_distance = None

        self._current_waypoint = None
        self._current_transform = None

        self.target_road_option = None
        self._next_waypoints = None
        self.target_waypoint = None
        self.local_transform = None # add 
        self._vehicle_controller = None
        self._global_plan = None
        self._pid_controller = None
        
        self.waypoints_queue = deque(maxlen=20000)  # queue with tuples of (waypoint, RoadOption) 10 km
        self.smooth_frenetpoints = deque(maxlen=20000) # queue with FrenetPoint
        
        self._max_distance = 50.0 # 最远规划距离50米，也就是最大速度不超过60km/s
        self._buffer_size = math.ceil(self._max_distance / agent._sampling_resolution) # 0.5m res 50m/0.5 向上取整
        self._waypoint_buffer = deque(maxlen = self._buffer_size)
        self._frenetpoints_tree_buffer = LocalRoadTreeDeque(maxlen = self._buffer_size) # 
        self._velocity_buffer = deque(maxlen = self._buffer_size)

        self._init_controller()  # initializing controller
        self.next_speed = -1

        # debug
        self._prediction_obj = PerceptPrediction(self._world, self._vehicle)
        # self._local_road_id = [] # 用于记录局部路径上的道路信息(road_id, section_id, lane_id)
        self._bias_rec = 0.1 # only for debug

    def refill_velocity_buffer(self):
        """
        重新填充速度buffer
        """
        pass

    def get_vehicle_frontcenter(self):
        """
        获取车辆前轴中心位置
        @param NONE
        @return location:Carla 3D vector
        """
        current_transform = self._vehicle.get_transform()
        location = current_transform.location
        yaw = current_transform.rotation.yaw * math.pi / 180
        location.x += math.cos(yaw) * self._axis_length / 2
        location.y += math.sin(yaw) * self._axis_length / 2
        return location

    def reset_vehicle(self):
        """Reset the ego-vehicle"""
        self._vehicle = None
        print("Resetting ego-vehicle!")

    def _init_controller(self):
        """
        Controller initialization.

        dt -- time difference between physics control in seconds.
        This is can be fixed from server side
        using the arguments -benchmark -fps=F, since dt = 1/F

        target_speed -- desired cruise speed in km/h

        min_distance -- minimum distance to remove waypoint from queue

        lateral_dict -- dictionary of arguments to setup the lateral PID controller
                            {'K_P':, 'K_D':, 'K_I':, 'dt'}

        longitudinal_dict -- dictionary of arguments to setup the longitudinal PID controller
                            {'K_P':, 'K_D':, 'K_I':, 'dt'}
        """
        # Default parameters
        self.args_lat_hw_dict = {
            'K_P': 0.75,
            'K_D': 0.02,
            'K_I': 0.4,
            'dt': 1.0 / self.FPS}
        self.args_lat_city_dict = {
            'K_P': 0.58,
            'K_D': 0.02,
            'K_I': 0.5,
            'dt': 1.0 / self.FPS}
        self.args_long_hw_dict = {
            'K_P': 0.37,
            'K_D': 0.024,
            'K_I': 0.032,
            'dt': 1.0 / self.FPS}
        self.args_long_city_dict = {
            'K_P': 0.15,
            'K_D': 0.05,
            'K_I': 0.07,
            'dt': 1.0 / self.FPS}

        self._current_waypoint = self._map.get_waypoint(self._vehicle.get_location())

        self._global_plan = False

        self._target_speed = self._vehicle.get_speed_limit()

        self._min_distance = self._sampling_resolution * 2

    def set_speed(self, speed):
        """
        Request new target speed.

            :param speed: new target speed in km/h
        """
        self._target_speed = speed

    def set_global_plan(self, current_plan):
        """
        将carla map的全局路径转换为pnc需要的全局路径：
        (transform, s, kappa, dappa)，保持 smooth_frenetpoints的序号与 current_plan一致
        """
        total_number = len(current_plan)
        change_lane_number = []
        for i in range(1, total_number):
            current_wp, _ = current_plan[i]
            previous_wp, _ = current_plan[i-1]
            # 两点距离，两点间角度变化
            p_c_dist, p_c_angle = compute_magnitude_angle(current_wp.transform.location, 
                                                                                                                      previous_wp.transform.location, 
                                                                                                                      previous_wp.transform.rotation.yaw) 
            diff_yaw = current_wp.transform.rotation.yaw - previous_wp.transform.rotation.yaw # 自有朝向变化

            delta = diff_yaw - p_c_angle # 
            delta = math.fabs(clip_angle(delta, -180, 180))
            # 转向变化过度：两点之间的夹角与两点朝向角变化的差值过大
            if delta > math.degrees(2 * self._sampling_resolution / 6.0) and p_c_dist > self._sampling_resolution / 2.0: # 最小转弯半径6.0米
                change_lane_number.append(i)

        # 前后选择10米重新计算location和rotation，10米是拍脑袋,假定的是换一个车道4米，
        mod_len = 10 
        jump_index = int(mod_len / self._sampling_resolution)
        calc_lane_number = []
        # 计算需要变换的起始点和结束点
        for index in change_lane_number:
            start_index = index - jump_index if index-jump_index>=0 else 0
            end_index = index+jump_index if index+jump_index<total_number else total_number - 1
            if calc_lane_number:
                if start_index < calc_lane_number[-1][1]:
                    start_index = calc_lane_number[-1][1]
                calc_lane_number.append((start_index, end_index))
            else:
                calc_lane_number.append((start_index, end_index)) 
        # print(calc_lane_number)
        # 先填充        
        for i, elem in enumerate(current_plan):
            if i>0:
                cur_loc = elem[0].transform.location
                pre_loc = current_plan[i-1][0].transform.location
                dist = compute_distance(cur_loc, pre_loc)
                if dist > self._sampling_resolution / 10:
                    self.waypoints_queue.append(elem)
                    # self.smooth_frenetpoints.append([elem[0].transform, elem[0].s, 0.0, 0.0])
                    self.smooth_frenetpoints.append(FrenetPoint(elem[0].transform, centre_s=elem[0].s, bias=0.0, kappa=0.0, dappa=0.0))
        # 计算bezier曲线
        for (si, ei) in calc_lane_number:
            # bezier
            loc0 = self.smooth_frenetpoints[si].transform.location
            yaw0 = self.smooth_frenetpoints[si].transform.rotation.yaw
            loc1 = self.smooth_frenetpoints[ei].transform.location
            yaw1 = self.smooth_frenetpoints[ei].transform.rotation.yaw
            number = ei - si + 1
            wave = GetWaveBezier(loc0.x, loc0.y, yaw0, loc1.x, loc1.y, yaw1, number)
            for i, (x,y) in enumerate(wave): # wave是bezier曲线
                self.smooth_frenetpoints[si+i].transform.location.x = float(x) #loc_xy[i][0]
                self.smooth_frenetpoints[si+i].transform.location.y = float(y) #loc_xy[i][0]
                if i > 0:
                    # 第一点朝向不变，第二点是1-2点线段的朝向
                    cur_wp = self.smooth_frenetpoints[si+i].transform
                    pre_wp = self.smooth_frenetpoints[si+i-1].transform
                    # dist, angle = compute_magnitude_angle(cur_wp.location, pre_wp.location, pre_wp.rotation.yaw)
                    dist, angle = compute_magnitude_angle(cur_wp.location, pre_wp.location, 0.0)
                    self.smooth_frenetpoints[si+i].transform.rotation.yaw = -angle
                    self.smooth_frenetpoints[si+i].s = self.smooth_frenetpoints[si+i-1].s + dist
        # 更新s、kappa(曲率 radians/radius)、dappa(曲率变化率)等
        for i, _ in enumerate(self.smooth_frenetpoints):
            if i == 0:
                self.smooth_frenetpoints[i].s = 0 # s
                self.smooth_frenetpoints[i].kappa = 0 # 曲率
                self.smooth_frenetpoints[i].dappa = 0 # 曲率变化率
            else: 
                cur_wp = self.smooth_frenetpoints[i].transform
                pre_wp = self.smooth_frenetpoints[i-1].transform
                dist, _ = compute_magnitude_angle(cur_wp.location, pre_wp.location, pre_wp.rotation.yaw)
                kappa = math.radians(cur_wp.rotation.yaw - pre_wp.rotation.yaw) / dist
                dappa = (kappa - self.smooth_frenetpoints[i-1].kappa) / dist
                self.smooth_frenetpoints[i].s = self.smooth_frenetpoints[i-1].s + dist # s
                self.smooth_frenetpoints[i].kappa = kappa # 曲率
                self.smooth_frenetpoints[i].dappa = dappa # 曲率变化率

        # for debug
        _tfs = []       
        for fnp in self.smooth_frenetpoints:
            _tfs.append(fnp.transform)
        draw_transforms(self._world, _tfs, time=-1)
        self._global_plan = True

    def get_incoming_waypoint_and_direction(self, steps=3):
        """
        Returns direction and waypoint at a distance ahead defined by the user.
            :param steps: number of steps to get the incoming waypoint.
        """
        if len(self.waypoints_queue) > steps:
            return self.waypoints_queue[steps]

        else:
            try:
                wpt, direction = self.waypoints_queue[-1]
                return wpt, direction
            except IndexError as i:
                print(i)
                return None, RoadOption.VOID
        return None, RoadOption.VOID

    # def local_trajection_extend(self):
    #     """
    #     不做任何检查，直接延伸局部路径，之后也直接延伸速度
    #     @param None
    #     @return None
    #     """
    #     front_location = self.get_vehicle_frontcenter() # 前轮中心
    #     _, current_index = self._frenetpoints_tree_buffer.query(front_location)

    #     if current_index > 0:
    #         for i in range(current_index):
    #             self._waypoint_buffer.popleft() # 清除局部路径中已经经过的点
    #             self._frenetpoints_tree_buffer.popleft() # 清除局部路径中已经经过的点

    #             if self.waypoints_queue and self.smooth_frenetpoints: # 全局路径仍然有点
    #                 (wp, ro) = self.waypoints_queue.popleft() # 全局路径中的当前点
    #                 global_cur_fnp = self.smooth_frenetpoints.popleft() # 全局路径中的当前点
                    
    #                 local_last_fnp = self._frenetpoints_tree_buffer[-1] # 局部路径最终点
    #                 bias = local_last_fnp.l # 相对全局路径参考点的偏移量 参见frenet坐标定义
    #                 """
    #                 only for debug
    #                 """
    #                 self._bias_rec = 0.1
    #                 bias = math.max(1.5, local_last_fnp.l + self._bias_rec)
    #                 """
    #                 only for debug
    #                 """                    

    #                 next_fnp = FrenetPoint(global_cur_fnp.transform, global_cur_fnp.s, bias, global_cur_fnp.kappa, global_cur_fnp.dappa)
    #                 print('center_location=', next_fnp.centre_location)
    #                 print('location = ', next_fnp.transform.location)
    #                 print()
    #                 self._waypoint_buffer.append((wp, ro)) # waypoint局部路径
    #                 self._frenetpoints_tree_buffer.append(next_fnp)                    
    #                 # self._velocity_buffer.append() # TODO 增加速度判断

    def check_traffic_rules(self):
        """
        检查路线是否符合交通规则
        @param None
        @return indexes : list of the index ?
        """
        for (i, fnp) in enumerate(self._frenetpoints_tree_buffer):
            if math.fabs(fnp.l) > 0.01: # 只有不在参考线上的点才需要检查
                # TODO need check

                pass 
        return
    
    def get_local_trajection(self, warning_plan_obs, collision_plan_obs):
        """
        1.按照车辆终点新加入一点：
            ———— 此处隐含了一个条件，即终点朝向和全局规划一致
        1.1 车辆终点在frenet坐标系下与全局规划路径的关系 l偏离

        2.从当前点按照全局路径点
        """
        front_location = self.get_vehicle_frontcenter() # 前轮中心
        _, current_index = self._frenetpoints_tree_buffer.query(front_location)
        # 之后可以把里面的顺序流程变成多个处理线程
        # 判断障碍物
        for cob in collision_plan_obs:
            # 障碍物距离过近，直接刹车
            for vertice in cob:
                if vertice.local_index * self._sampling_resolution < get_speed(self._vehicle)*1.0 \
                    or vertice.local_index*self._sampling_resolution < 5.0:
                    return VehicleAction.BRAKE
            pass

        # 先移除旧点，并直接延长轨迹
        if current_index > 0:
            for i in range(current_index):
                (wp, _) = self._waypoint_buffer.popleft()
                if self.waypoints_queue:
                    (wp, ro) = self.waypoints_queue.popleft()
                    self._waypoint_buffer.append((wp, ro))

                self._frenetpoints_tree_buffer.popleft()
                # 增加buffer taosheng 6.11
                local_fp = self._frenetpoints_tree_buffer[-1]
                global_fp = self.smooth_frenetpoints.popleft()

                #for debug
                bias = min(local_fp.l + self._bias_rec, 1.5)
                print('bias=', bias)
                #for debug over

                new_fp = FrenetPoint(global_fp.transform, global_fp.s, bias, global_fp.kappa, global_fp.dappa)
                print('centre_location:', new_fp.centre_location)
                print('local_location:', new_fp.transform.location)
                print('')

                if self.smooth_frenetpoints:
                    self._frenetpoints_tree_buffer.append(new_fp)
        
        return VehicleAction.STRAIGHT

    def check_collision(self, dist_plan_obs, ttc_plan_obs):
        """
        检查当前轨迹是否与障碍物碰撞
        @param dist_plan_obs: 用于规划的静态障碍物列表
        @param ttc_plan_obs: 用于规划的动态障碍物列表
        @return collision_plan_obs: 可能发生碰撞的规划障碍物列表
        @return warning_plan_obs: 距离过近的规划障碍物列表
        """
        collision_plan_obs = []
        warning_plan_obs = []
        for ob_box in dist_plan_obs:
            warning_tag = False
            for vertice in ob_box:
                if math.fabs(vertice.l) < 2.0: # 碰撞，不需要再检测其他点
                    collision_plan_obs.append(ob_box)
                    break
                if math.fabs(vertice.l) < 5.0: # 距离过近 测试是否减速
                    warning_tag = True
            else:
                if warning_tag:
                    warning_plan_obs.append(ob_box)
        return collision_plan_obs, warning_plan_obs

    def sort_obs(self, static_obs, moving_obs):
        """
        计算障碍物顶点坐标与局部路径 self._frenetpoints_buffer最近的距离和索引号
        并根据索引号对障碍物顶点进行排序
        @param static_obs: 感知输出的静态障碍物数据
        @param moving_obs: 感知输出的动态障碍物数据
        @return static_plan_obs: 用于规划的静态障碍物数据
        @return moving_plan_obs: 用于规划的动态障碍物数据
        """
        static_plan_obs = []
        moving_plan_obs = []
        for so in static_obs:
            # 障碍物中心tf
            so_transform = so.get_transform()
            # 障碍物各个顶点与局部路径的距离
            bbox = so.bounding_box
            ob_locs = bbox.get_world_vertices(so_transform) # 将bbox顶点位置转换到地图坐标系
            dists, indexes = self._frenetpoints_tree_buffer.query(ob_locs) # 利用KD树查找bbox各个顶点对应的行车路径对应距离与序号
            plan_ob_box = []
            for i in range(len(ob_locs)):
                fnp = self._frenetpoints_tree_buffer[indexes[i]]
                wp_transform = fnp.transform
                _, angle = compute_magnitude_angle(ob_locs[i], wp_transform.location, wp_transform.rotation.yaw)
                if 0<angle<180:
                    dists[i] *= -1.0
                plan_box_v = PlanBoxVertice(indexes[i], dists[i], ob_locs[i])
                plan_ob_box.append(plan_box_v)
            plan_ob_box.sort(key=lambda vertice: vertice.local_index)
            static_plan_obs.append(plan_ob_box) 
        return static_plan_obs, moving_plan_obs

    # def filter_obs(self, dist_obs, filter_dist, ttc_obs, filter_ttc):
    #     """
    #     根据距离、排列障碍物
    #     """
    #     dist_collision_obs = []
    #     for (_, ob) in dist_obs:
    #         # 障碍物中心tf
    #         transform = ob.get_transform()
    #         # 障碍物各个顶点与局部路径的距离
    #         bbox = ob.bounding_box
    #         ob_locs = bbox.get_world_vertices(transform)
    #         dists, indexes = self._frenetpoints_tree_buffer.query(ob_locs)
    #         for d in dists:
    #             if d < 2.0: # 距离车道中心距离在2.0米（车宽+定位误差）
    #                 dist_collision_obs.append(ob)
    #     return dist_collision_obs

    def stop_and_brake(self):
        control = carla.VehicleControl()
        control.steer = 0.0
        control.throttle = 0.0
        control.brake = 1.0
        control.hand_brake = False
        control.manual_gear_shift = False
        return control

    def init_local_trace(self):        
        if not self._waypoint_buffer: # 如果局部路径（_waypoint_buffer）为空，填充路径
            for i in range(self._buffer_size):
                if self.waypoints_queue:
                    self._frenetpoints_tree_buffer.append(
                        self.smooth_frenetpoints.popleft())

                    (wp, ro) = self.waypoints_queue.popleft()
                    self._waypoint_buffer.append((wp, ro))

                    # road_id = wp.road_id
                    # section_id = wp.section_id
                    # lane_id = wp.lane_id
                    # if (road_id, section_id, lane_id) not in self._local_road_id:
                    #     self._local_road_id.append((road_id, section_id, lane_id))
                else:
                    break

    def multitrace_generate(self):
        """

        """
        right_turn = True
        left_turn = True
        none_turn = True

        front_location = self.get_vehicle_frontcenter() # 前轮中心
        _, current_index = self._frenetpoints_tree_buffer.query(front_location)

        if current_index > 0:
            for i in range(current_index):
                self._waypoint_buffer.popleft() # 清除局部路径中已经经过的点
                self._frenetpoints_tree_buffer.popleft() # 清除局部路径中已经经过的点

                # TODO 根据局部路径中转换得到的waypoint判断是否可以左右变道
                # 新加的点怎么放？怎么判断多次变道?
                if right_turn:
                    pass
                if left_turn:
                    pass
                if none_turn:
                    pass

        return
    
    def run_step(self, static_obs, moving_obs, target_speed=None, debug=True):
        """
        https://blog.csdn.net/u013468614/article/details/103518011
        δ(k)=θe(k)+arctan(λe(k)/v(k))，
        其中，θe(k)为k时刻的航向角偏差，计算方法：目标点方向 - 按现在控制参数车辆到达点的方向
        e(k)为根据上图几何学计算的横向跟踪误差，计算方法：按照当前控制参数车辆到达点 与 目标点 之间的距离
        λ为需要调节的参数，v为无人车当前速度，δ(k)为前轮偏角
        需要注意的是：因为方向盘转角速度是有限制的，因此δ(k)的变化率不能太大，却δ(k)是有最大值的

        stanley的思想是：当我以delta(k)角运动时，我可以完全补偿完所有的横向误差（假定到达点和当前点的朝向一致，即在一条直线上）

        带前馈的stanly模型：
        1.计算当前位置上的横向误差
        2.根据stanley算法计算前轮转角 delta0
        3.计算dt能到达的前馈点
        4.计算前馈点朝向与当前点朝向的差 delta1
        """
        control = carla.VehicleControl(steer=0, throttle=0) # init

        # 如果到达终点，则停止
        if len(self.waypoints_queue) == 0:
            control = self.stop_and_brake()
            return control

        # 如果局部路径（_waypoint_buffer）为空，说明故障，停车
        if self._frenetpoints_tree_buffer:
            self._frenetpoints_tree_buffer.set_tree()
        else:
            print('get something wrong.')
            return self.stop_and_brake 

        # 对感知障碍物根据在局部路径上的距离排序
        static_plan_obs, moving_plan_obs = self.sort_obs(static_obs, moving_obs)
        # 判断障碍物是否会发生碰撞
        collision_plan_obs, warning_plan_obs = self.check_collision(static_plan_obs, moving_plan_obs)

        # 计算前轮中心，以便执行stanley控制
        front_location = self.get_vehicle_frontcenter() # 前轮中心
        # 规划新的局部路径
        vehicle_action = self.get_local_trajection(warning_plan_obs, collision_plan_obs)
        if vehicle_action == VehicleAction.BRAKE:
            print('get emegency brake!')
            return self.stop_and_brake()

        # 以下为控制逻辑
        print('try to control')
        self._current_waypoint, _ = self._waypoint_buffer[0] # 旧点已经被抛弃
        fnp = self._frenetpoints_tree_buffer[0]
        self._current_transform = fnp.transform

        # for debug
        draw_waypoints(self._vehicle.get_world(), [self._current_waypoint], z=2.5) # 
        car_transform = self._vehicle.get_transform()
        car_transform.location = front_location

        # 获取本车当前状态
        speed = get_speed(self._vehicle) # 速度
        acc =get_acceleration(self._vehicle) # 加速度
        current_steer = self._vehicle.get_control().steer # 当前转角比例
        draw_speed_with_tf(self._vehicle.get_world(), car_transform, speed=speed, color=carla.Color(0,255,0), z=2.5) #

        # 计算当前状态下可能到达的target点，后期stanley中可以考虑增加预瞄点
        dist = speed*self._dt + acc / 2.0 * self._dt * self._dt + self._axis_length / 2 # 增加了半个轴长的预瞄
        ind = math.ceil(dist / self._sampling_resolution) # 向上取整
        if ind > len(self._waypoint_buffer):
            ind = -1
        self.target_waypoint, self.target_road_option = self._waypoint_buffer[ind]
        fnp = self._frenetpoints_tree_buffer[ind]
        self.target_transform = fnp.transform

        # 获取本车位置
        yaw = self._vehicle.get_transform().rotation.yaw * math.pi / 180.0
        
        # 初始化stanley横向控制器
        stanley_controller = StanleyController(front_location, yaw=yaw, v=speed, dt = self._dt, k=0.3)
        # 计算控制角度
        next_steer, lat_dist_error = stanley_controller.run_step(self._current_transform, self._max_steer_angle)
        # 考虑车辆角度变化的快慢能力，限制转角速度
        next_steer = np.clip(next_steer, -1, 1)
        d_steer = 0.05
        if next_steer > current_steer + d_steer:
            next_steer = current_steer + d_steer
        elif next_steer < current_steer - d_steer:
            next_steer = current_steer - d_steer

        # 准备进行纵向速度控制
        # pid参数选择
        if target_speed > 50:
            args_lat = self.args_lat_hw_dict
            args_long = self.args_long_hw_dict
        else:
            args_lat = self.args_lat_city_dict
            args_long = self.args_long_city_dict

        self._pid_controller = VehiclePIDController(self._vehicle,
                                                    args_lateral=args_lat,
                                                    args_longitudinal=args_long) # 横向pid参数，纵向pid参数

        # 计算3s内的道路曲率，从而限定预期的目标
        pre_aim = max(get_speed(self._vehicle) * 3.0, 9.0) # 预瞄位置：最小转弯半径6.0，按照6/0.7计算
        pre_indexes = math.ceil(pre_aim / self._sampling_resolution)
        max_curv = 0.0
        for i in range(1, min(len(self._waypoint_buffer), pre_indexes)):
            wp0, _ = self._waypoint_buffer[i-1]
            wp1, _ = self._waypoint_buffer[i] 
            curv = math.fabs(get_curvature(wp0, wp1)) # 不考虑正负
            if curv > max_curv:
                max_curv = curv
        # 根据转角、横向误差、最大曲率来确定目标速度
        if math.fabs(next_steer) > 0.7 or lat_dist_error > 3.0 or max_curv > 1.0/8.0:
            self._target_speed = 3.0
        elif math.fabs(next_steer) > 0.4 or lat_dist_error > 1.5 or max_curv > 1.0/15.0:
            self._target_speed = 5.0
        elif math.fabs(next_steer) > 0.2 or lat_dist_error >  0.5 or max_curv > 1.0/25.0:
            self._target_speed = 6.0
        else:
            self._target_speed = 7.0
        control = self._pid_controller.run_step(self._target_speed, self.target_transform)

        # for debug 显示角度控制值
        control.steer = next_steer
        car_transform.rotation.yaw += next_steer*self._max_steer_angle
        draw_speed_with_tf(self._vehicle.get_world(), car_transform, speed=speed, color=carla.Color(255,255,0), z=2.5) #

        # for debug 显示局部规划路径
        if debug:
            tfs = [fnp.transform for fnp in self._frenetpoints_tree_buffer]
            if tfs:
                draw_transforms(self._world, tfs, time=0.1, thickness=0.5, color=carla.Color(0,0,255))
        return control
